#include "ros/ros.h"
#include "tf/transform_listener.h"

#include <iostream>
#include <string>
#include <cmath>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>

#include <std_msgs/Header.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32MultiArray.h>

#include "mpccbf_line.h"
#include "mpccbf.h"

const int CTRL_IDLE = 1;
const int CTRL_INIT = 2;
const int CTRL_RUN = 3;
nav_msgs::Path predicted_path;
std_msgs::Float64 msg_cost;
int ctrl_state = 0;
int angular_cnt = 0;
int rgl = 0;
mpccbf::SystemState system_state;
mpccbf::TrajectoryPoint target_point;
std::vector<std::pair<double,double>> route_points;
std::vector<mpccbf::TrajectoryPoint> planning_published_trajectory;
sensor_msgs::Imu::ConstPtr g_imu_data;
geometry_msgs::Twist twist;
nav_msgs::Odometry distance;
float yaw_w = 0.0;
double vx= 0.0,vy= 0.0;
// void imuCallback(const sensor_msgs::Imu::ConstPtr& msg){

// }

void routeCallback(const std_msgs::Float32MultiArray& msg) {
    int size = msg.data.size() / 3;
    route_points.clear();
    for (int i = 0; i < size; ++i) {
        route_points.push_back(std::make_pair(msg.data[i * 3], msg.data[i * 3 + 1]));
    }
}

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg){   //统一坐标系下
    system_state.v_x =  msg->twist.twist.linear.x;
    system_state.v_y =  msg->twist.twist.linear.y;
    vx =  msg->twist.twist.linear.x;
    vy =  msg->twist.twist.linear.y;
    yaw_w  = tf::getYaw(msg->pose.pose.orientation);
}

void poseCallback(const geometry_msgs::Pose::ConstPtr& msg) {
    system_state.x = msg->position.x;
    system_state.y = msg->position.y;
  
    tf::Quaternion q;
    tf::quaternionMsgToTF(msg->orientation,q);
    tf::Matrix3x3(q).getRPY(system_state.roll,system_state.pitch,system_state.yaw);
   // system_state.yaw *= -1;
    system_state.heading = system_state.yaw;
    // std::cout<<"system_state.x :"<<system_state.x<<std::endl;
    // std::cout<<"system_state.y :"<<system_state.y<<std::endl;
}


void obstCallback(const std_msgs::Float32MultiArray& msg) {
    system_state.obs_x = msg.data[0];
    system_state.obs_y = msg.data[1];
}

void tgtCallback(const std_msgs::Float32MultiArray& msg) {
    if (msg.data.size() != 2) return;
    target_point.x = 6.9;
    target_point.y = -3.4;
    target_point.heading = 0 ;
    //target_point.heading = angular_cnt * M_PI/2;
    //angular_cnt++;
    //angular_cnt = angular_cnt%4;
    
}
void rglCallback(const std_msgs::Bool& msg) {
    rgl = (msg.data==false);
    
}

void cmdCallback(const std_msgs::String& msg) {
	ctrl_state = CTRL_INIT;
    if (msg.data == "1") {
        if (ctrl_state == CTRL_IDLE) {
            ctrl_state = CTRL_INIT;
        }
    }
    else if (msg.data == "0") {
        ctrl_state = CTRL_IDLE;
    }
} 

std::vector<uint8_t> get_bag(float v_x,float v_y,float w){
    std::vector<uint8_t> data(24);
    float p1 = 0.0;
    float p2 = 0.0;
    float p3 = 0.0;
    memcpy(data.data(), &v_x , 4);
    memcpy(data.data()+4, &v_y , 4);
    memcpy(data.data()+8, &w , 4);
    memcpy(data.data()+12, &p1 , 4);
    memcpy(data.data()+16, &p2 , 4);
    memcpy(data.data()+20, &p3 , 4);
    return data;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "mpccbf_node");
    ros::NodeHandle nh;
    ROS_INFO("Doing mpc-cbf");

    tf::TransformListener listener;

    ros::Subscriber sub_odom = nh.subscribe("/car/car_state", 10000 ,odomCallback);
    ros::Subscriber sub_pose = nh.subscribe("/location"    , 10000 ,poseCallback);
    ros::Subscriber sub_cmd  = nh.subscribe("ctrl_cmd"     , 10 , cmdCallback);
    ros::Subscriber sub_tgt  = nh.subscribe("/mpc_target_point", 100 , tgtCallback);
    ros::Subscriber sub_rgl  = nh.subscribe("/rgl", 100 , rglCallback);
    ros::Subscriber sub_route = nh.subscribe("/mpc_route"  , 10 , routeCallback);
    // ros::Subscriber sub_obst  = n.subscribe("/polaris/obstacle", 10, obstCallback);

    ros::Publisher pub_path = nh.advertise<nav_msgs::Path>("/predicted_path", 100);
    ros::Publisher pub_gazebo = nh.advertise<geometry_msgs::Twist>("/car/cmd_vel_car", 100);
    ros::Publisher pub_cost = nh.advertise<std_msgs::Float64>("/mpc_cost", 100);
    mpccbf::CtrlCmd cmd;
    std::unique_ptr<mpccbf::MPCCBF> mpc_cbf = std::make_unique<mpccbf::MPCCBF>();
    ros::Publisher pub_dis = nh.advertise<nav_msgs::Odometry>("/mpc_distance", 100);
    ctrl_state = CTRL_IDLE;
        target_point.x = 6.9;
        target_point.y = -3.4;
        target_point.heading = 0 ;
    ros::Rate loop_rate(20);
    while(ros::ok()){
        ros::spinOnce();
        double cost = 0;
        distance.pose.pose.position.x = system_state.x - target_point.x;
        distance.pose.pose.position.y = system_state.y - target_point.y;
        

        if(ctrl_state == CTRL_INIT){
            std::vector<double> headings;
            std::vector<double> accumulated_s;
            std::unique_ptr<mpccbf::ReferenceLine> reference_line = std::make_unique<mpccbf::ReferenceLine>(route_points);
            reference_line->ComputePathProfile(&headings,&accumulated_s);
            for(std::size_t i = 0; i<headings.size(); ++i){
                mpccbf::TrajectoryPoint trajectory_pt;
                trajectory_pt.x = route_points[i].first;
                trajectory_pt.y = route_points[i].second;
                trajectory_pt.heading = 0;
                trajectory_pt.v_x = (route_points[i+1].first-route_points[i].first)/0.01;
                trajectory_pt.v_y = (route_points[i+1].second-route_points[i].second)/0.01;
                planning_published_trajectory.push_back(trajectory_pt);
            }
            ctrl_state = CTRL_RUN;
        }
        else if(ctrl_state == CTRL_RUN){
            try{
                if(std::isnan(system_state.yaw)){
                    throw 0;
                }
                std::vector<double> result = mpc_cbf->Solve(system_state,planning_published_trajectory,target_point);
                if(result.size() < 3){
                    throw 0;
                }
                else {
                    cmd.steer_target = result[0];
                    cmd.acc_x = result[1]*0.1+vx;
                    cmd.acc_y = result[2]*0.1+vy;
                    cost = result[3];
                    twist.linear.x = cmd.acc_x;
                    twist.linear.y = cmd.acc_y;
                    twist.angular.z = 0;
                }

            }
            catch(...){
                ROS_INFO("Error Occurred");
                cmd.acc_x = 0;
                cmd.acc_y = 0;
                cmd.steer_target = 0;
                twist.linear.x = cmd.acc_x;
                twist.linear.y = cmd.acc_y;
                twist.angular.z = cmd.steer_target;      
            }
        }
if((system_state.x - target_point.x)*(system_state.x - target_point.x)<=0.02&&(system_state.y - target_point.y)*(system_state.y - target_point.y)<=0.02)
        {
                cmd.acc_x = 0;
                cmd.acc_y = 0;
                cmd.steer_target = 0;
                twist.linear.x = cmd.acc_x;
                twist.linear.y = cmd.acc_y;
                twist.angular.z = cmd.steer_target;
        }
if((system_state.x - 6.3)*(system_state.x - 6.3)<=0.02&&rgl == 0)
{
		cmd.acc_x = 0;
                cmd.acc_y = 0;
                cmd.steer_target = 0;
                twist.linear.x = cmd.acc_x;
                twist.linear.y = cmd.acc_y;
                twist.angular.z = cmd.steer_target;
}        
        
        
        pub_gazebo.publish(twist);
        msg_cost.data = cost;
        pub_cost.publish(msg_cost);
        pub_dis.publish(distance);

        
    }
    return 0;

}

